import pyzed.sl as sl
import time
import redis


if __name__ == "__main__":
    r = redis.Redis(db=0)
    r.set('camera_1_command', 'off')

    while True:
        if r.get('camera_1_command').decode("utf-8") == 'off':

            init_params = sl.InitParameters()
            init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_ULTRA
            init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD1080
            init_params.coordinate_units = sl.UNIT.UNIT_MILLIMETER  # UNIT_METER
            init_params.depth_minimum_distance = 300
            init_params.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_LEFT_HANDED_Y_UP
            init_params.camera_linux_id = 0

            zed = sl.Camera()

            err = zed.open(init_params)
            if err != sl.ERROR_CODE.SUCCESS:
                exit(1)

            runtime_parameters = sl.RuntimeParameters()
            runtime_parameters.sensing_mode = sl.SENSING_MODE.SENSING_MODE_FILL

            transform = sl.Transform()
            tracking_params = sl.PositionalTrackingParameters(transform)
            zed.enable_positional_tracking(tracking_params)

            image = sl.Mat()
            depth = sl.Mat()
            pose = sl.Pose()
            py_translation = sl.Translation()

            point_cloud = sl.Mat()

            irene = 0
            while True:
                try:
                    irene = irene + 1

                    if irene == 10:
                        irene = 0
                        if r.get('camera_1_command').decode("utf-8") != 'on':
                            break
                    err = zed.grab(runtime_parameters)
                    if err == sl.ERROR_CODE.SUCCESS:
                        zed.retrieve_image(image, sl.VIEW.VIEW_LEFT)
                        zed.retrieve_measure(point_cloud, sl.MEASURE.MEASURE_XYZRGBA)

                        zed.get_position(pose)

                        tx = round(pose.get_translation(py_translation).get()[0], 3)
                        ty = round(pose.get_translation(py_translation).get()[1], 3)
                        tz = round(pose.get_translation(py_translation).get()[2], 3)

                        py_orientation = sl.Orientation()
                        ox = round(pose.get_orientation(py_orientation).get()[0], 3)
                        oy = round(pose.get_orientation(py_orientation).get()[1], 3)
                        oz = round(pose.get_orientation(py_orientation).get()[2], 3)

                        print([ox, oy, oz])


                        img = image.get_data()
                        depth = point_cloud.get_data()
                        r.mset({'image': img.tobytes(),
                                'depth': depth.tobytes(),
                                'ox': ox,
                                'oy': oy,
                                'oz': oz,
                                'tx': tx,
                                'ty': ty,
                                'tz': tz
                                })
                    else:
                        print(err)

                except KeyboardInterrupt:
                    print('quit')
                    break
                # time.sleep(0.002)

            zed.close()
        else:

            time.sleep(0.5)
